#pragma once

#include <eigen3/Eigen/Eigen>
#include <ceres/ceres.h>
#include <list>

using std::list;

class Area
{
public:
    std::list<Eigen::Vector2d> vertex;
    double avg_height;
    Eigen::ArrayXXi station_mask;

    void init(const double *vtx, const int vtx_n, const int *st_mask, const int st_mask_n, const double h)
    {
        // init vertexs for area
        if (vtx != nullptr && st_mask != nullptr)
        {
            for (int idx = 0; idx != vtx_n; ++idx)
            {
                vertex.push_back(Eigen::Vector2d(vtx[idx * 2 + 0], vtx[idx * 2 + 1]));
            }

            // init height for this area
            avg_height = h;

            // init station mask for this area
            station_mask = Eigen::ArrayXXi::Zero(st_mask_n, 1);
            for (int idx = 0; idx != st_mask_n; ++idx)
            {
                station_mask(idx, 0) = st_mask[idx];
            }
        }
        else
        {
            // init height for this area
            avg_height = h;
            station_mask = Eigen::ArrayXXi::Zero(st_mask_n, 1);
        }
    }

    bool is_in_area(const double &e, const double &n) const
    {
        
        bool is_in_flag = true;
        auto itr1 = vertex.begin();
        auto itr2 = vertex.begin();
        itr2++;
        const Eigen::Vector2d pt(n, e);
        int i = 0;
        while (itr1 != vertex.end())
        {
            const Eigen::Vector2d &p1 = *itr1;
            if (itr2 == vertex.end())
                itr2 = vertex.begin();
            const Eigen::Vector2d &p2 = *itr2;

            Eigen::Vector2d v1 = p2 - p1;
            Eigen::Vector2d v2 = pt - p1;

            double cross = v1[0] * v2[1] - v1[1] * v2[0];
            // std::cerr << "Input N=" << pt[0] << ", E=" << pt[1] << std::endl;
            // std::cerr << "Pt1   N=" << p1[0] << ", E=" << p1[1] << std::endl;
            // std::cerr << "Pt2   N=" << p2[0] << ", E=" << p2[1] << std::endl;
            // std::cerr << "CROSS=" << cross << std::endl; 

            if (cross < 0)
            {
                is_in_flag = false;
                break;
            }

            itr1++;
            itr2++;
        }
        // std::cerr << (is_in_flag ? "TRUE" : "FALSE") << std::endl;
        return is_in_flag;
    }
};

// cost functor for error computing
struct CostFunctor
{
    const double _east, _north, _dist;
    CostFunctor(double north, double east, double dist) : _north(north), _east(east), _dist(dist) {}
    template <typename T>
    bool operator()(const T *const p, T *r) const
    {
        r[0] = _dist - ceres::sqrt((p[0] - _north) * (p[0] - _north) + (p[1] - _east) * (p[1] - _east));
        return true;
    }
};

class Locator
{
public:
    size_t num_stations;

    // the station positions in enu coord
    Eigen::ArrayXXd station_pos;
    // mask the stations not in use in current area
    Eigen::ArrayXXi station_mask;
    // station base
    Eigen::Array2d station_pos_base;

    // the plane distance
    Eigen::ArrayXXd dist_plane;
    // the distance msg mask, some of the station may not reporting there position
    Eigen::ArrayXXi dist_mask;

    // the delta height for each station
    Eigen::ArrayXXd height_delta;

    // record the last distance
    Eigen::ArrayXXd dist_last;

    // loacation solved
    double pos[2];

    // flags
    int area_set;

    void init(const double *station_pos_matrix, const double *station_base, const int n_station)
    {
        num_stations = n_station;

        station_pos = Eigen::ArrayXXd::Zero(num_stations, 3);
        for (int s_idx = 0; s_idx != num_stations; ++s_idx)
        {
            station_pos(s_idx, 0) = station_pos_matrix[s_idx * 3 + 0];
            station_pos(s_idx, 1) = station_pos_matrix[s_idx * 3 + 1];
            station_pos(s_idx, 2) = station_pos_matrix[s_idx * 3 + 2];
        }
        station_pos_base = Eigen::Array2d::Zero();
        station_pos_base(0, 0) = station_base[0];
        station_pos_base(1, 0) = station_base[1];

        station_mask = Eigen::ArrayXXi::Ones(num_stations, 1);
        dist_plane = Eigen::ArrayXXd::Zero(num_stations, 1);
        dist_mask = Eigen::ArrayXXi::Ones(num_stations, 1);
        height_delta = Eigen::ArrayXXd::Zero(num_stations, 1);
        height_delta.setConstant(3.35 - 3.27);

        dist_last = Eigen::ArrayXXd::Zero(num_stations, 1);

        pos[0] = 0.0;
        pos[1] = 0.0;
        area_set = 0;
    }

    bool update_dist(const double d_in[], const int d_mask[], const size_t d_num)
    {
        if (d_in != nullptr && d_mask != nullptr && d_num == num_stations)
        {
            // backup the last distance info
            dist_last = dist_plane;
            for (size_t idx = 0; idx != d_num; ++idx)
            {
                // mask the dist value that report no valid distance
                dist_mask(idx, 0) = d_mask[idx];
                dist_plane(idx, 0) = std::sqrt(d_in[idx]*d_in[idx] - height_delta(idx, 0)*height_delta(idx, 0));
            }
            return true;
        }
        std::cerr << "[WARNING: In update_dist@Locator] no input data is valid!" << std::endl;
        return false;
    }

    void set_area(const Area &area, int id)
    {
        height_delta = station_pos.col(2) - area.avg_height;
        station_mask = area.station_mask;
        area_set = id;
    }

    bool solve(double &east, double &north, double &loss)
    {
        // select all valid masks, return if failed!
        Eigen::ArrayXXi valid_mask = station_mask * dist_mask;
        const int num_valid_stations = static_cast<int>(valid_mask.sum());
        if (num_valid_stations < 3)
        {
            north = 0.0;
            east = 0.0;
            std::cerr << "[WARNING: In solve@Locator] the valid station is not enough [" << num_valid_stations << "]!" << std::endl;
            return false;
        }

        // create a problem
        ceres::Problem problem;

        // fill in Residual blocks to ceres solver
        for (int row = 0; row != valid_mask.rows(); ++row)
        {
            if (valid_mask(row, 0) != 0)
            {
                const double sp_n = station_pos(row, 0); // - station_pos_base(0, 0);
                const double sp_e = station_pos(row, 1); // - station_pos_base(1, 0);
                const double sp_d = dist_plane(row, 0);

                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<CostFunctor, 1, 2>(
                        new CostFunctor(sp_n, sp_e, sp_d)),
                    // new ceres::CauchyLoss(0.5),
                    nullptr,
                    pos);
            }
        }

        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = false;
        // options.max_num_iterations = 50;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);

        north = pos[0] + station_pos_base(0, 0);
        east = pos[1] + station_pos_base(1, 0);

        if (summary.termination_type != ceres::CONVERGENCE)
        {
            std::cerr << "[WARNING: In solve@Locator] with " << num_valid_stations << " stations" << std::endl;
            // std::cerr << summary.BriefReport() << std::endl;
            std::cerr << "The station infomation listed below:" << std::endl;
            for (int row = 0; row != valid_mask.rows(); ++row)
            {
                if (valid_mask(row, 0) != 0)
                {
                    std::cerr << "A" << row << ": DIS=" << dist_plane(row, 0) << std::endl;
                }
            }
            std::cerr << std::endl;
            return false;
        }

        return true;
    }
};

bool init_senario(const int n_st, const double *st, const double *a_vtx, const int *a_mask, const double * st_base, const double *a_h);

bool update_senario(const double *dist_in, const int *dist_mask, const int dist_num, double gnss_east, double gnss_north, double *e, double *n, int *area);